#!/usr/bin/env python3

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node

def generate_launch_description():
    # 声明启动参数
    device_arg = DeclareLaunchArgument(
        'device',
        default_value='/dev/ttyS0',
        description='串口设备路径'
    )
    
    baudrate_arg = DeclareLaunchArgument(
        'baudrate',
        default_value='115200',
        description='串口波特率'
    )
    
    frame_id_arg = DeclareLaunchArgument(
        'frame_id',
        default_value='imu_link',
        description='IMU坐标系ID'
    )
    
    publish_rate_arg = DeclareLaunchArgument(
        'publish_rate',
        default_value='100.0',
        description='发布频率(Hz)'
    )
    
    verbose_arg = DeclareLaunchArgument(
        'verbose',
        default_value='true',
        description='是否启用详细输出'
    )

    # IMU发布者节点
    imu_publisher_node = Node(
        package='imu_sensor',
        executable='imu_publisher',
        name='imu_publisher',
        output='screen',
        parameters=[{
            'device': LaunchConfiguration('device'),
            'baudrate': LaunchConfiguration('baudrate'),
            'frame_id': LaunchConfiguration('frame_id'),
            'publish_rate': LaunchConfiguration('publish_rate'),
            'verbose': LaunchConfiguration('verbose'),
        }],
        remappings=[
            ('/imu/data', '/imu/data'),
            ('/imu/mag', '/imu/mag'),
        ]
    )

    # IMU订阅者节点
    imu_subscriber_node = Node(
        package='imu_sensor',
        executable='imu_subscriber',
        name='imu_subscriber',
        output='screen',
        parameters=[{
            'imu_topic': '/imu/data',
            'mag_topic': '/imu/mag',
            'print_interval': 2.0,  # 每2秒打印一次统计信息
            'verbose': LaunchConfiguration('verbose'),
        }]
    )

    # 启动信息
    launch_info = LogInfo(
        msg=[
            '启动IMU演示系统\n',
            '设备: ', LaunchConfiguration('device'), '\n',
            '波特率: ', LaunchConfiguration('baudrate'), '\n',
            '坐标系: ', LaunchConfiguration('frame_id'), '\n',
            '发布频率: ', LaunchConfiguration('publish_rate'), ' Hz\n',
            '详细输出: ', LaunchConfiguration('verbose'), '\n',
            '同时启动发布者和订阅者节点'
        ]
    )

    return LaunchDescription([
        device_arg,
        baudrate_arg,
        frame_id_arg,
        publish_rate_arg,
        verbose_arg,
        launch_info,
        imu_publisher_node,
        imu_subscriber_node,
    ])
